/*
 * cordic.c
 *
 *  Created on: Apr 10, 2024
 *      Author: Yiwen Zhu
 */
#include "main.h"
#include "my_def.h"
#include "math.h"
#include "cordic.h"
#include "arm_math.h"

#define q31_to_f32(x)	ldexp( (int32_t)x, -31 )
// value_q31 = f32_to_q31(value_f32)

int f32_to_q31(float input){
	const float Q31_MAX_F = 0x0.FFFFFFp0F;
	const float Q31_MIN_F = -1.0F;
	float clamped = fmaxf( fminf(input,Q31_MAX_F), Q31_MIN_F );
	return (int)roundf(scalbnf(clamped,31));
}


float cordic_q31_sinf(float x){
	CORDIC_ConfigTypeDef sConfig;
	int32_t	input_q31 = f32_to_q31( fmod(x,2.0f*PI) / (2.0f*PI) ) <<1;
	int32_t output_q31;

	sConfig.Function = CORDIC_FUNCTION_SINE;
	sConfig.Precision = CORDIC_PRECISION_6CYCLES;
	sConfig.Scale = CORDIC_SCALE_0;
	sConfig.NbWrite = CORDIC_NBWRITE_1;
	sConfig.NbRead = CORDIC_NBREAD_1;
	sConfig.InSize = CORDIC_INSIZE_32BITS;
	sConfig.OutSize = CORDIC_INSIZE_32BITS;
	HAL_CORDIC_Configure(&hcordic,&sConfig);
	HAL_CORDIC_CalculateZO(&hcordic,&input_q31,&output_q31,1,0);
	return q31_to_f32(output_q31);
}

float cordic_q31_cosf(float x){
	CORDIC_ConfigTypeDef sConfig;
	int32_t	input_q31 = f32_to_q31( fmod(x,2.0f*PI) / (2.0f*PI) ) <<1;
	int32_t output_q31;

	sConfig.Function = CORDIC_FUNCTION_COSINE;
	sConfig.Precision = CORDIC_PRECISION_6CYCLES;
	sConfig.Scale = CORDIC_SCALE_0;
	sConfig.NbWrite = CORDIC_NBWRITE_1;
	sConfig.NbRead = CORDIC_NBREAD_1;
	sConfig.InSize = CORDIC_INSIZE_32BITS;
	sConfig.OutSize = CORDIC_INSIZE_32BITS;
	HAL_CORDIC_Configure(&hcordic,&sConfig);
	HAL_CORDIC_CalculateZO(&hcordic,&input_q31,&output_q31,1,0);
	return q31_to_f32(output_q31);
}

int f32_to_q15(float input){
	float src[1]={input};
	q15_t dst[1]={0};

	arm_float_to_q15(src,dst,1);
	return dst[0];
}

float q15_to_f32(int32_t input){
	q15_t src[1]={input};
	float dst[1]={0};
	arm_q15_to_float(src,dst,1);
	return dst[0];
}

float cordic_q15_sinf(float x){
	CORDIC_ConfigTypeDef sConfig;
	int32_t input_q15 = f32_to_q31( fmod(x,2.0f*PI) / (2.0f*PI) ) <<1;
	int32_t output_q15;

	sConfig.Function = CORDIC_FUNCTION_SINE;
	sConfig.Precision = CORDIC_PRECISION_6CYCLES;
	sConfig.Scale = CORDIC_SCALE_0;
	sConfig.NbWrite = CORDIC_NBWRITE_1;
	sConfig.NbRead = CORDIC_NBREAD_1;
	sConfig.InSize = CORDIC_INSIZE_16BITS;
	sConfig.OutSize = CORDIC_INSIZE_16BITS;
	HAL_CORDIC_Configure(&hcordic,&sConfig);
	HAL_CORDIC_CalculateZO(&hcordic,&input_q15,&output_q15,1,0);
	return q15_to_f32(output_q15);
}

float cordic_q15_cosf(float x){
	CORDIC_ConfigTypeDef sConfig;
	int32_t	input_q15 = f32_to_q15( fmod(x,2.0f*PI) / (2.0f*PI) ) <<1;
	int32_t   output_q15;

	sConfig.Function = CORDIC_FUNCTION_COSINE;
	sConfig.Precision = CORDIC_PRECISION_6CYCLES;
	sConfig.Scale = CORDIC_SCALE_0;
	sConfig.NbWrite = CORDIC_NBWRITE_1;
	sConfig.NbRead = CORDIC_NBREAD_1;
	sConfig.InSize = CORDIC_INSIZE_16BITS;
	sConfig.OutSize = CORDIC_INSIZE_16BITS;
	HAL_CORDIC_Configure(&hcordic,&sConfig);
	HAL_CORDIC_CalculateZO(&hcordic,&input_q15,&output_q15,1,0);
	return q15_to_f32(output_q15);
}


